#include "protocolhandler.h"
#include <QDataStream>
#include <QIODevice>

ProtocolHandler::ProtocolHandler(QObject *parent)
    : QObject(parent)
{
}

QByteArray ProtocolHandler::buildCommandFrame(quint8 sourceId, quint8 destId, quint8 frameCounter,
                                               quint8 command, const QByteArray &params, quint16 imageClarity)
{
    QByteArray frame;
    QDataStream stream(&frame, QIODevice::WriteOnly);
    stream.setByteOrder(QDataStream::LittleEndian);
    
    // Fixed frame length: 13 bytes total, frame length field = 12
    // Frame structure: StartMarker(2) + Length(1) + SourceID(1) + DestID(1) + FrameID(1) + 
    //                  FrameCounter(1) + Command(1) + Params(2, padded with 0) + ImageClarity(2) + Checksum(1)
    constexpr quint8 FIXED_FRAME_LENGTH = 12;  // Total 13 bytes - 1
    constexpr int PARAM_FIELD_SIZE = 2;  // Fixed parameter field size
    
    // Write frame start marker (0xAAAA, little-endian)
    stream << FRAME_START_MARKER;
    
    // Write fixed frame length
    stream << FIXED_FRAME_LENGTH;
    
    // Write source ID
    stream << sourceId;
    
    // Write destination ID
    stream << destId;
    
    // Write frame ID (0x00 for single-frame data)
    stream << static_cast<quint8>(0x00);
    
    // Write frame counter
    stream << frameCounter;
    
    // Write command
    stream << command;
    
    // Write parameters (fixed 2 bytes, pad with 0 if needed)
    if (params.size() >= PARAM_FIELD_SIZE) {
        // If params is 2 or more bytes, use first 2 bytes
        stream.writeRawData(params.constData(), PARAM_FIELD_SIZE);
    } else if (params.size() == 1) {
        // If params is 1 byte, write it and pad with 0
        stream.writeRawData(params.constData(), 1);
        stream << static_cast<quint8>(0x00);
    } else {
        // If no params, write 2 zero bytes
        stream << static_cast<quint8>(0x00);
        stream << static_cast<quint8>(0x00);
    }
    
    // Write image clarity (UINT16, little-endian)
    stream << imageClarity;
    
    // Calculate and append checksum (sum of all bytes except checksum itself, low 8 bits)
    quint8 checksum = calculateChecksum(frame);
    stream << checksum;
    
    return frame;
}

void ProtocolHandler::processIncomingData(const QByteArray &data)
{
    // Append new data to receive buffer
    m_receiveBuffer.append(data);
    
    // Process all complete frames in the buffer
    while (m_receiveBuffer.size() >= 3) {  // Minimum: start marker (2) + length (1)
        // Search for frame start marker 0xAAAA
        int startIndex = -1;
        for (int i = 0; i <= m_receiveBuffer.size() - 2; ++i) {
            quint16 marker = (static_cast<quint8>(m_receiveBuffer[i]) |
                            (static_cast<quint8>(m_receiveBuffer[i + 1]) << 8));
            if (marker == FRAME_START_MARKER) {
                startIndex = i;
                break;
            }
        }
        
        // If no start marker found, clear buffer (invalid data)
        if (startIndex == -1) {
            m_receiveBuffer.clear();
            emit frameError("No frame start marker found in buffer");
            break;
        }
        
        // Remove data before start marker
        if (startIndex > 0) {
            m_receiveBuffer.remove(0, startIndex);
        }
        
        // Check if we have enough data to read frame length
        if (m_receiveBuffer.size() < 3) {
            break;  // Wait for more data
        }
        
        // Extract frame length (total frame bytes - 1)
        quint8 frameLength = static_cast<quint8>(m_receiveBuffer[2]);
        int totalFrameSize = frameLength + 1;
        
        // Check if we have the complete frame
        if (m_receiveBuffer.size() < totalFrameSize) {
            break;  // Wait for more data
        }
        
        // Extract the complete frame
        QByteArray frame = m_receiveBuffer.left(totalFrameSize);
        
        // Validate the frame
        if (validateFrame(frame)) {
            // Parse the status frame
            parseStatusFrame(frame);
        } else {
            emit frameError("Frame validation failed (invalid checksum)");
        }
        
        // Remove processed frame from buffer
        m_receiveBuffer.remove(0, totalFrameSize);
    }
}

quint8 ProtocolHandler::calculateChecksum(const QByteArray &data)
{
    quint32 sum = 0;
    for (int i = 0; i < data.size(); ++i) {
        sum += static_cast<quint8>(data[i]);
    }
    // Return low 8 bits
    return static_cast<quint8>(sum & 0xFF);
}

bool ProtocolHandler::validateFrame(const QByteArray &frame)
{
    if (frame.size() < 4) {  // Minimum frame size
        return false;
    }
    
    // Extract checksum from last byte
    quint8 receivedChecksum = static_cast<quint8>(frame[frame.size() - 1]);
    
    // Calculate checksum for all bytes except the checksum itself
    QByteArray dataWithoutChecksum = frame.left(frame.size() - 1);
    quint8 calculatedChecksum = calculateChecksum(dataWithoutChecksum);
    
    return receivedChecksum == calculatedChecksum;
}

void ProtocolHandler::parseStatusFrame(const QByteArray &frame)
{
    // Frame structure: StartMarker(2) + Length(1) + SourceID(1) + DestID(1) + FrameID(1) + FrameCounter(1) + Command(1) + WorkStatus(1) + CurrentStep(2) + TotalTravel(2) + Checksum(1)
    // Minimum size: 13 bytes
    if (frame.size() < 13) {
        emit frameError("Status frame too short");
        return;
    }
    
    QDataStream stream(frame);
    stream.setByteOrder(QDataStream::LittleEndian);
    
    // Skip start marker (2 bytes)
    quint16 startMarker;
    stream >> startMarker;
    
    // Skip length (1 byte)
    quint8 length;
    stream >> length;
    
    // Read source ID
    quint8 sourceId;
    stream >> sourceId;
    
    // Skip destination ID (1 byte)
    quint8 destId;
    stream >> destId;
    
    // Skip frame ID (1 byte)
    quint8 frameId;
    stream >> frameId;
    
    // Skip frame counter (1 byte)
    quint8 frameCounter;
    stream >> frameCounter;
    
    // Read command echo
    quint8 command;
    stream >> command;
    
    // Read work status byte
    quint8 workStatus;
    stream >> workStatus;
    
    // Read current focus step (INT16, little-endian)
    qint16 currentStep;
    stream >> currentStep;
    
    // Read motor total travel (INT16, little-endian)
    qint16 totalTravel;
    stream >> totalTravel;
    
    // Emit signal with parsed data
    emit frameReceived(sourceId, command, workStatus, currentStep, totalTravel);
}
